module MainMotorsP {
	provides interface Init;
	provides interface MainMotorControl;
	provides interface MainMotorDebug;

	uses interface MotorControl as LeftMotorControl;
	uses interface MotorControl as RightMotorControl;

	uses interface HplAtm128Interrupt as LeftMotorInt;
	uses interface HplAtm128Interrupt as RightMotorInt;

	uses interface Timer<TMilli>;
	uses interface Timer<TMilli> as ControllerTimer;
	uses interface Leds;
}

implementation {

	enum {
		IDLE = 0,
		GO_PWM,
		GO_PID,
	};
	
	uint8_t state = IDLE;

	int8_t dirleft = 0, dirright = 0;
	uint16_t left = 0, right = 0; // rotation counters
	uint16_t dsleft = 0, dsright = 0; // desired speed
	uint16_t csleft = 0, csright = 0; // current speed

	int16_t Cp = 2000, Ci = 1300;
	uint16_t dt = 50;
	
	int16_t Ileft = 0, Iright = 0;
	int16_t Pleft = 0, Pright = 0;

//	int16_t desired;

	command error_t Init.init() {
		call LeftMotorInt.disable();
		call LeftMotorInt.clear();
		call LeftMotorInt.edge(TRUE);	
		call LeftMotorInt.enable();
		
		call RightMotorInt.disable();
		call RightMotorInt.clear();
		call RightMotorInt.edge(TRUE);	
		call RightMotorInt.enable();
		return SUCCESS;
	}
	
	
	command error_t MainMotorControl.setPWM(int16_t speed1, int16_t speed2, uint16_t time) {
		atomic state = GO_PWM;
		call LeftMotorControl.setSpeed(speed1);
		call RightMotorControl.setSpeed(speed2);
		call Timer.startOneShot(time);
		return SUCCESS;
	}

	void do_pid() {
		int32_t pwmleft, pwmright;
		atomic {
			csleft = left ; left = 0;
			csright = right ; right = 0;
		}
		Pleft = (dsleft - csleft);
		Pright = (dsright - csright);
		
		Ileft += Pleft;
		Iright += Pright;

		pwmleft = dirleft * ((int32_t)Pleft * Cp + (int32_t)Ileft * Ci);
		if (pwmleft > 0x7FFF)
			pwmleft = 0x7FFF ;
		else if (pwmleft < -0x7FFF)
			pwmleft = -0x7FFF;

		pwmright = dirright * ((int32_t)Pright * Cp + (int32_t)Iright * Ci);
		if (pwmright > 0x7FFF)
			pwmright = 0x7FFF;
		else if (pwmright < -0x7FFF)
			pwmright = -0x7FFF;

		call LeftMotorControl.setSpeed((int16_t)pwmleft);
		call RightMotorControl.setSpeed((int16_t)pwmright);
		
		signal MainMotorDebug.controllerTick(csleft, csright, Ileft, Iright, (int16_t)pwmleft, (int16_t)pwmright);
	}

	event void ControllerTimer.fired() {
		do_pid();
	}
	
	command error_t MainMotorControl.setPID(int16_t speed1, int16_t speed2, uint16_t time) {
		bool continous = FALSE;
		atomic {
			if (state == GO_PID)
				continous = TRUE;
			state = GO_PID;
		}
		call Timer.startOneShot(time);
		if (!continous)
			call ControllerTimer.startPeriodic(dt);
		
		if (speed1 > 0) {
			dsleft = speed1 >> 10;
			dirleft = 1;
		} else if (speed1 < 0) {
			dsleft = (-speed1) >> 10;
			dirleft = -1;
		} else {
			dsleft = 0;
			dirleft = 0;
		}
		
		if (speed2 > 0) {
			dsright = speed2 >> 10;
			dirright = 1;
		} else if (speed2 < 0){
			dsright = (-speed2) >> 10;
			dirright = -1;
		} else {
			dsright = 0;
			dirright = 0;
		}

		if (!continous) {
			Ileft = 0; Iright = 0;
			atomic {left = 0; right = 0;}
			csleft = csright = 0;
			do_pid();
		}
		return SUCCESS;
	}

	command void MainMotorControl.setParameters(int16_t cp, int16_t ci, uint16_t deltat) {
		Cp = cp;
		Ci = ci;
		dt = deltat;
	}	
	
	async event void LeftMotorInt.fired() {
		atomic {
			left++;
		}
		call Leds.led1Toggle();
	}

	async event void RightMotorInt.fired() {
		atomic {
			right++;
		}
		call Leds.led2Toggle();
	}

	event void Timer.fired() {
		call ControllerTimer.stop();
		call LeftMotorControl.setSpeed(0);
		call RightMotorControl.setSpeed(0);
		atomic {
			state = IDLE;
		}
	}

	default event void MainMotorDebug.controllerTick(uint16_t _csleft, uint16_t _csright, int16_t _Ileft, int16_t _Iright, int16_t _pwmleft, int16_t _pwmright) {
	
	}

}
